
#include <esp32-hal-gpio.h>
#include <TFT_eSPI.h>
#include <Arduino.h>
#include "AS5600.h"
#include <USBCDC.h>

#define _3PI_2 4.71238898038f
#define Pin_SCL 0
#define Pin_SDA 1
int Sensor_DIR=-1;    //传感器方向
int Motor_PP=7;    //电机极对数
int pwmA = 2;
int pwmB = 3;
int pwmC = 4;
float voltage_power_supply=8.0;
float Ualpha,Ubeta=0,Ua=0,Ub=0,Uc=0;
float zero_electric_angle;
float dc_a=0,dc_b=0,dc_c=0;
float motor_target = 2.0;
float ratio_scale = 0.5;
int PP, DIR;

Sensor_AS5600 S0=Sensor_AS5600(0);
TwoWire S0_I2C = TwoWire(0);
TFT_eSPI tft = TFT_eSPI();  // Invoke custom library

#define _constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))

// 设置PWM到控制器输出
void setPwm(float Ua, float Ub, float Uc) {
  // 限制上限
  Ua = _constrain(Ua, 0.0f, voltage_power_supply);
  Ub = _constrain(Ub, 0.0f, voltage_power_supply);
  Uc = _constrain(Uc, 0.0f, voltage_power_supply);
  // 计算占空比
  // 限制占空比从0到1
  dc_a = _constrain(Ua / voltage_power_supply, 0.0f , 1.0f ) * ratio_scale;
  dc_b = _constrain(Ub / voltage_power_supply, 0.0f , 1.0f ) * ratio_scale;
  dc_c = _constrain(Uc / voltage_power_supply, 0.0f , 1.0f ) * ratio_scale;

  //写入PWM到PWM 0 1 2 通道
  ledcWrite(0, dc_a*255);
  ledcWrite(1, dc_b*255);
  ledcWrite(2, dc_c*255);
}

// 归一化角度到 [0,2PI]
float _normalizeAngle(float angle){
  float a = fmod(angle, 2*PI);   //取余运算可以用于归一化，列出特殊值例子算便知
  return a >= 0 ? a : (a + 2*PI);  
}

// 电角度求解
// float _electricalAngle(float shaft_angle, int pole_pairs) {
//   return (shaft_angle * pole_pairs);
// }
float _electricalAngle(){
  return  _normalizeAngle((float)(DIR *  PP) * S0.getMechanicalAngle()-zero_electric_angle);
}

void setTorque(float Uq,float angle_el) {
  S0.Sensor_update(); //更新传感器数值
  Uq=_constrain(Uq,-(voltage_power_supply)/2,(voltage_power_supply)/2);
  float Ud=0;
  angle_el = _normalizeAngle(angle_el);
  // 帕克逆变换
  Ualpha =  -Uq*sin(angle_el); 
  Ubeta =   Uq*cos(angle_el); 

  // 克拉克逆变换
  Ua = Ualpha + voltage_power_supply/2;
  Ub = (sqrt(3)*Ubeta-Ualpha)/2 + voltage_power_supply/2;
  Uc = (-Ualpha-sqrt(3)*Ubeta)/2 + voltage_power_supply/2;
  setPwm(Ua,Ub,Uc);
}


void DFOC_alignSensor(int _PP,int _DIR)
{ 
  PP=_PP;
  DIR=_DIR;
  setTorque(3, _3PI_2);  //起劲
  delay(1000);
  S0.Sensor_update();  //更新角度，方便下面电角度读取
  zero_electric_angle=_electricalAngle();
  setTorque(0, _3PI_2);  //松劲（解除校准）
  Serial.print("0电角度：");
  Serial.println(zero_electric_angle);
}

float DFOC_M0_Angle()
{
  return DIR*S0.getAngle();
}

void setup() {
  Serial.begin(460800);
  /* tft init */
  tft.init();
  tft.setRotation(1);
  tft.fillScreen(TFT_WHITE);
  tft.setTextColor(TFT_BLACK);
  /* motor init */
  pinMode(pwmA, OUTPUT);
  pinMode(pwmB, OUTPUT);
  pinMode(pwmC, OUTPUT);
  ledcSetup(0, 30000, 8);  //pwm频道, 频率, 精度
  ledcSetup(1, 30000, 8);  //pwm频道, 频率, 精度
  ledcSetup(2, 30000, 8);  //pwm频道, 频率, 精度
  ledcAttachPin(pwmA, 0);
  ledcAttachPin(pwmB, 1);
  ledcAttachPin(pwmC, 2);
  Serial.println("完成PWM初始化设置");
  /* sensor init */
  S0_I2C.begin(Pin_SDA, Pin_SCL, 400000UL);
  S0.Sensor_init(&S0_I2C);   //初始化编码器0
  Serial.println("编码器加载完毕");

  //PID 加载
  // vel_loop_M0 = PIDController{.P = 2, .I = 0, .D = 0, .ramp = 100000, .limit = voltage_power_supply/2};
  // PIDController vel_loop_M0(2, 0, 0, 100000, voltage_power_supply/2);
  DFOC_alignSensor(Motor_PP, Sensor_DIR);
}

void loop() 
{
  //输出角度值
  float Kp=0.2;
  float Sensor_Angle=DFOC_M0_Angle();
  float Uq = Kp*(motor_target - Sensor_DIR*Sensor_Angle)*180/PI;
  Serial.println("Angle:" + String(Sensor_Angle, 2));
  Serial.println("Uq:" + String(Uq, 2));
  tft.fillScreen(TFT_WHITE);
  tft.setCursor(0, 5, 1);
  tft.println(" Angle:" + String(Sensor_Angle, 2));
  tft.println(" Uq:" + String(Uq, 2));
  setTorque(Uq, _electricalAngle());   //位置闭环
}
